#include <L298.h>
L298::L298(char in1, char in2, char in3, char in4, char ena, char enb)
    : IN1(in1), IN2(in2), IN3(in3), IN4(in4), ENA(ena), ENB(enb) {
    pinMode(in1, OUTPUT);
    pinMode(in2, OUTPUT);
    pinMode(in3, OUTPUT);
    pinMode(in4, OUTPUT);
    analogWrite(ena, 0);
    analogWrite(enb, 0);
}

void L298::feed(char channel, int speed) {
    if (0 == channel) {
        analogWrite(ENA, abs(speed));
        if (speed >= 0) {
            digitalWrite(IN1, 1);
            digitalWrite(IN2, 0);
        } else {
            digitalWrite(IN1, 0);
            digitalWrite(IN2, 1);
        }
    } else if (1 == channel) {
        analogWrite(ENB, abs(speed));
        if (speed >= 0) {
            digitalWrite(IN3, 1);
            digitalWrite(IN4, 0);
        } else {
            digitalWrite(IN3, 0);
            digitalWrite(IN4, 1);
        }
    }
}

void L298::brake(char channel) {
    if (0 == channel) {
        digitalWrite(IN1, 0);
        digitalWrite(IN2, 0);
        analogWrite(ENA, 255);
    } else if (1 == channel) {
        digitalWrite(IN3, 0);
        digitalWrite(IN4, 0);
        analogWrite(ENB, 255);
    }
}

void L298::run(float dir, float oil) {
    // next 3 lines code is useless but for robustness
    if (oil > 1) oil = 1;
    else if (oil < 0) oil = 0;
    if (abs(dir) > 360) dir = fmod(dir, 360);

    if (dir <= 180) {
        if (dir <= 90) {
            feed(0, 255 * oil);
            feed(1, (dir - 45) * 510.0/90.0 * oil);
        } else {
            feed(1, 255 * oil);
            feed(0, (135 - dir) * 510.0/90.0 * oil);
        }
    } else {
        if (dir <= 270) {
            feed(1, -255 * oil);
            feed(0, (225 - dir) * 510.0/90.0 * oil);
        } else {
            feed(0, -255 * oil);
            feed(1, (dir - 315) * 510.0/90.0 * oil);
        }
    }
}
